{{ Receiver Test }}

CON
  _clkmode = xtal1 + pll16x                                               
  _xinfreq = 5_000_000

  FORWARD  = 1
  REVERSE  = 2
  LEFT     = 3
  RIGHT    = 4
  STOP     = 5

OBJ
  xbee : "XBee_Object"
  pst  : "Parallax Serial Terminal"
  functions : "Functions"
  
VAR
  long stack[9]
  byte arm_servo
  
PUB Main
  pst.start(115200)
  waitcnt(clkfreq + cnt)
  functions.initialize
  pst.str(String("Starting to receive packets from transmitter.",13))
  xbee.start(4,3,0,9600)
  pst.str(String("Received the following: ",13))
  arm_servo := 120
  coginit(4, functions.Drive(STOP), @stack)
  'coginit(5, MoveMotor(20,120), @stack)
  repeat
    Parse
    
PUB Parse
  pst.dec(xbee.rx)
  pst.str(String(13))
  if (xbee.rx == 0)
    pst.str(String("Received: 0",13))
    pst.str(String("Doing nothing!",13))
    cogstop(4)
    coginit(4, functions.Drive(STOP), @stack)
  else
    if (xbee.rx == 1)
      cogstop(4)
      coginit(4, functions.Drive(REVERSE), @stack)
    if (xbee.rx == 2)
      cogstop(4)
      coginit(4, functions.Drive(FORWARD), @stack) 
    if (xbee.rx == 4)
      cogstop(4)
      coginit(4, functions.Drive(RIGHT), @stack)
    if (xbee.rx == 5)
      cogstop(4)
      coginit(4, functions.Drive(LEFT), @stack)
    {if (xbee.rx == 6)
      cogstop(5)
      cogstop(6)
      coginit(5, MoveMotor(20, 120), @stack+100)
      waitcnt(clkfreq + cnt)
      coginit(6, MoveMotor(21, 90), @stack+2000)
    if (xbee.rx == 7)
      cogstop(5)
      cogstop(6)
      coginit(6, MoveMotor(21, 120), @stack+2000)
      waitcnt(clkfreq + cnt)
      coginit(5, MoveMotor(20, 220), @stack+100)}
        
PUB MoveMotor(Pin, position)                              
  dira[Pin]~~                                   
  repeat                                        
    outa[Pin]~~                                 
    waitcnt((clkfreq/100_000)*position+cnt)    
    outa[Pin]~                                 
    waitcnt(clkfreq/100+cnt)